/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#ifndef KALMAN_2D_H
#define KALMAN_2D_H

#include <stdint.h>
#include <string.h>


typedef struct kalman_2d
{

    float z[2][2];
    float x_hat_predict[2][2];     // 先验估计
    float x_hat[2][2];             // 后验估计
    float x_hat_last[2][2];        // 前一个 后验估计
    float A[2][2];              // X 转移矩阵
    float H[2][2];              // Z 转移矩阵
    float Q[2][2];              // 过程噪声
    float R[2][2];              // 测量噪声
    float P[2][2];              // 状态协方差矩阵
    float P_predict[2][2];      // 先验估计误差协方差矩阵
    float P_predict_last[2][2]; // 先验估计误差协方差矩阵
    float P_hat[2][2];          // 后验估计误差协方差矩阵
    float K[2][2];              // 卡尔曼增益
    float I[2][2];              // 单位矩阵

} kalman_2d_t;

void kalman_2d_init_vel_pos(kalman_2d_t *k, float q1, float q2, float r1, float r2, float dt);
void kalman_2d_update_vel_pos(kalman_2d_t *k, float position, float velocity);
#endif // KALMAN_2D_H
